/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.CriteriaCollection;
import edu.wpi.first.wpilibj.image.NIVision;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.commands.DetectTargets;
import edu.wpi.first.wpilibj.templates.commands.MoveCam;

/**
 *
 * @author Robotics
 */
public class Camera extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    final double VIEW_ANGLE = 43.5; //For M1011
    final int X_IMAGE_RESOLUTION = 320;
    final int Y_IMAGE_RESOLUTION = 240;
    final double PI = 3.14159265359;

    //limits for the targets
    final int RECTANGULARITY_LIMIT = 40;
    final int ASPECT_RATIO_LIMIT = 55;

    //score limits for detemring hot targets
    final int TAPE_WIDTH_LIMIT = 50;
    final int VERT_SCORE_LIMIT = 50;
    final int LR_SCORE_LIMIT = 50;

    //Smallest area of particles to be processed
    final int MINIMUM_AREA = 150;

    //Max number of partcles to process
    final int MAX_NUM_PARTICLES = 8;

    //The axis camera object (connected to switch)
    AxisCamera camera;

    //The crteria for doing the particle filter operation
    CriteriaCollection cc;

    //test values for dashboard
    boolean horizontalTarget = false;
    boolean verticalTarget = false;
    double horizontalCenterX = 0;
    double horizontalCenterY = 0;
    double verticalCenterX = 0;
    double verticalCenterY = 0;
    double horizontalLength = 0;
    double verticalLength = 0;
    double horizontalDistance = 0;
    double verticalDistance = 0;

    //Testing values for writing to the FTP 
    /*boolean verticalImageTrip = false;
     boolean horizontalImageTrip = false;
     int horizontalPictureNumber = 1;
     int verticalPictureNumber = 1;
     int horizontalTargetCountz = 0;
     int verticalTargetCountz = 0;*/
    private static final double MIDPOINT = 0.5;
    private Servo servoX;
    private Servo servoY;

    //scores used for target detemrination 
    public class Scores {

        double rectangularity;
        double verticalAspectRatio;
        double horizontalAspectRatio;
    }

    //various aspect of the targets used in determining targets
    public class TargetAspects {

        int verticalIndex;
        int horizontalIndex;
        boolean Hot;
        double totalScore;
        double leftScore;
        double rightScore;
        double tapeWidthScore;
        double verticalScore;
    }

    public Camera() {
        servoX = new Servo(RobotMap.XCAMSERVO_PWM);
        servoY = new Servo(RobotMap.SIDECAR_SLOT2, RobotMap.YCAMSERVO_PWM);

        servoX.set(MIDPOINT);
        servoY.set(MIDPOINT);

        camera = AxisCamera.getInstance("10.6.14.11");
    }

    public void moveCam(Joystick stick) {
        servoX.set(stick.getX());
        servoY.set(stick.getY());
    }

    //pre: a report about the particle
    //post: returns the rectangularity of the paricle based on the bounding box drawn around it. 
    double scoreRectangularity(ParticleAnalysisReport report) {
        if (report.boundingRectWidth * report.boundingRectHeight == 0) {
            return 0;
        } else {
            return 100 * report.particleArea / (report.boundingRectWidth * report.boundingRectHeight);
        }
    }

    /**
     * Converts a ratio with ideal value of 1 to a score. The resulting function
     * is piecewise linear going from (0,0) to (1,100) to (2,0) and is 0 for all
     * inputs outside the range 0-2
     */
    double ratioToScore(double ratio) {
        return (Math.max(0, Math.min(100 * (1 - Math.abs(1 - ratio)), 100)));
    }
//pre: The image to use in processing, a report about the particle being processed, an int to refer to the particle, a boolean to determine if the target is vertical or not
    //post: returns the distance to the target

    double computeDistance(BinaryImage image, ParticleAnalysisReport report, int particleNumber, boolean isVertical) throws NIVisionException {
        double rectLong, height;
        double targetHeight;
        rectLong = NIVision.MeasureParticle(image.image, particleNumber, false, NIVision.MeasurementType.IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE);
        //using the smaller of the estimated rectangle long side and the bounding rectangle height results in better performance
        //on skewed rectangles
        System.out.println("Rect long: " + rectLong + " Particle Number: " + particleNumber);
        //height = Math.min(report.boundingRectHeight, rectLong);
        targetHeight = isVertical ? 32 : 23.5;
        if (isVertical) {
            return Y_IMAGE_RESOLUTION * targetHeight / (rectLong * 12 * 2 * Math.tan(VIEW_ANGLE * Math.PI / (180 * 2)));
        }
        return X_IMAGE_RESOLUTION * targetHeight / (rectLong * 12 * 2 * Math.tan((VIEW_ANGLE * 3.0 / 4.0) * Math.PI / (180 * 2)));
    }
//pre: image to process, report about the particle to process, int to refer to the particle, boolean to tell if the target is vertical or not
    //post: returns the aspect ratio of the target

    public double scoreAspectRatio(BinaryImage image, ParticleAnalysisReport report, int particleNum, boolean isVertical) throws NIVisionException {
        double rectLong, rectShort, aspectRatio, idealAspectRatio;

        rectLong = NIVision.MeasureParticle(image.image, particleNum, false, NIVision.MeasurementType.IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE);
        rectShort = NIVision.MeasureParticle(image.image, particleNum, false, NIVision.MeasurementType.IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE);
        if (isVertical) {
            idealAspectRatio = (4.0 / 32);
        } else {
            idealAspectRatio = (23.5 / 4);
        }

        if (report.boundingRectWidth < report.boundingRectHeight) {
            aspectRatio = ratioToScore((rectShort / rectLong) / idealAspectRatio);
        } else {
            aspectRatio = ratioToScore((rectLong / rectShort) / idealAspectRatio);
        }
        return aspectRatio;
    }
//pre: scores for a target and boolean to tell if the target is vertical or not
    //post: based on the scores the potential target has, returns if it is an actual target

    boolean scoreCompare(Scores scores, boolean isVertical) {
        boolean isTarget = true;

        isTarget &= scores.rectangularity > RECTANGULARITY_LIMIT;
        if (isVertical) {
            isTarget &= scores.verticalAspectRatio > ASPECT_RATIO_LIMIT;
        } else {
            isTarget &= scores.horizontalAspectRatio > ASPECT_RATIO_LIMIT;
        }
        return isTarget;
    }    //post: updates the dashboard values 

    public void updateDashboard() {
        SmartDashboard.putNumber("Horizontal CenterX", horizontalCenterX);
        SmartDashboard.putNumber("Horizontal CenterY", horizontalCenterY);
        SmartDashboard.putNumber("Horizontal Length", horizontalLength);
        SmartDashboard.putNumber("Hot Target Distance", horizontalDistance);//SmartDashboard.putNumber("Horizontal Distance", horizontalDistance);
        SmartDashboard.putBoolean("Hot Target Detected", horizontalTarget);
    }

    //MAIN
    public double findTargets() throws NIVisionException, AxisCameraException{
        updateDashboard();
        TargetAspects target = new TargetAspects();
        double distance = 0;
        int verticalTargets[] = new int[MAX_NUM_PARTICLES];
        int horizontalTargets[] = new int[MAX_NUM_PARTICLES];
        int verticalTargetCount, horizontalTargetCount;

        cc = new CriteriaCollection();
        cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_AREA, MINIMUM_AREA, 65535, false);//creates a new criteria collection with a criterion for larger than the minimum area particles

        ColorImage image = camera.getImage();

        BinaryImage thresholdImage = image.thresholdHSV(215, 255, 100, 255, 100, 255);//filter out non-red particles
        BinaryImage filteredImage = thresholdImage.particleFilter(cc);//filter out small particle using the criteria collection

        Scores scores[] = new Scores[filteredImage.getNumberParticles()];
        horizontalTargetCount = verticalTargetCount = 0;

        if (filteredImage.getNumberParticles() > 0) {//goes through the particles in the current image
            for (int i = 0; i < MAX_NUM_PARTICLES && i < filteredImage.getNumberParticles(); i++) {
                ParticleAnalysisReport report = filteredImage.getParticleAnalysisReport(i);//gets a report for the image
                scores[i] = new Scores();

                scores[i].rectangularity = scoreRectangularity(report);//sets the scores for the current particle being processed
                scores[i].verticalAspectRatio = scoreAspectRatio(filteredImage, report, i, true);
                scores[i].horizontalAspectRatio = scoreAspectRatio(filteredImage, report, i, false);
                if (scoreCompare(scores[i], false)) {//if its a horizontal target
                    horizontalTarget = true;//TEST
                    horizontalCenterX = report.center_mass_x;
                    horizontalCenterY = report.center_mass_y;
                    horizontalDistance = computeDistance(filteredImage, report, i, false);
                        //For writing to the FTP
                        /*if (!horizontalImageTrip) {
                     image.write("/HorizontalCameraImage" + horizontalPictureNumber + ".bmp");
                     thresholdImage.write("/HorizontalThresholdImage" + horizontalPictureNumber + ".bmp");
                     filteredImage.write("/HorizontalFilteredImage" + horizontalPictureNumber + ".bmp");
                     horizontalPictureNumber++;
                     if (horizontalPictureNumber > 3) {
                     horizontalImageTrip = true;
                     }
                     }*/
                    //horizontalTargetCountz++;
                    updateDashboard();
                    System.out.println("particle " + (i + 1) + " is a Horizontal Target centerX: " + report.center_mass_x + " centerY: " + report.center_mass_y);
                    horizontalTargets[horizontalTargetCount++] = i;
                } else if (scoreCompare(scores[i], true)) {//if its a vertical target
                    verticalTarget = true;//TEST
                    verticalCenterX = report.center_mass_x;
                    verticalCenterY = report.center_mass_y;
                    verticalDistance = computeDistance(filteredImage, report, i, true);
                        //For writing to the FTP
                        /*if (!verticalImageTrip) {
                     image.write("/VerticalCameraImage" + verticalPictureNumber + ".bmp");
                     thresholdImage.write("/VerticalThresholdImage" + verticalPictureNumber + ".bmp");
                     filteredImage.write("/VerticalFilteredImage" + verticalPictureNumber + ".bmp");
                     verticalPictureNumber++;
                     if (verticalPictureNumber > 3) {
                     verticalImageTrip = true;
                     }
                     }*/
                    //verticalTargetCountz++;
                    updateDashboard();
                    System.out.println("particle: " + (i + 1) + " is a Vertical target centerX: " + report.center_mass_x + "venterY: " + report.center_mass_y);
                    verticalTargets[verticalTargetCount++] = i;
                } else {//if neither horizontal or vertical
                    updateDashboard();
                    System.out.println("particle: " + (i + 1) + "is not a Target centerX: " + report.center_mass_x + "centerY: " + report.center_mass_y);
                }
                System.out.println("rect: " + scores[i].rectangularity + "ARHoriz: " + scores[i].horizontalAspectRatio);
                System.out.println("ARVert: " + scores[i].verticalAspectRatio);
            }
            horizontalTarget = false;
            updateDashboard();
            target.totalScore = target.leftScore = target.rightScore = target.tapeWidthScore = target.verticalScore = 0;
            target.verticalIndex = verticalTargets[0];
            for (int i = 0; i < verticalTargetCount; i++) {
                ParticleAnalysisReport verticalReport = filteredImage.getParticleAnalysisReport(verticalTargets[i]);
                for (int j = 0; j < horizontalTargetCount; j++) {
                    ParticleAnalysisReport horizontalReport = filteredImage.getParticleAnalysisReport(horizontalTargets[j]);
                    double horizWidth, horizHeight, vertWidth, leftScore, rightScore, tapeWidthScore, verticalScore, total;
                    //Measure equivalent rectangle sides for use in score calculation
                    horizWidth = NIVision.MeasureParticle(filteredImage.image, horizontalTargets[j], false, NIVision.MeasurementType.IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE);
                    vertWidth = NIVision.MeasureParticle(filteredImage.image, verticalTargets[i], false, NIVision.MeasurementType.IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE);
                    horizHeight = NIVision.MeasureParticle(filteredImage.image, horizontalTargets[j], false, NIVision.MeasurementType.IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE);
                    horizontalLength = horizWidth;//TEST
                    verticalLength = vertWidth;
                    updateDashboard();
                    //Determine if the horizontal target is in the expected location to the left of the vertical target
                    leftScore = ratioToScore(1.2 * (verticalReport.boundingRectLeft - horizontalReport.center_mass_x) / horizWidth);
                    //Determine if the horizontal target is in the expected location to the right of the  vertical target
                    rightScore = ratioToScore(1.2 * (horizontalReport.center_mass_x - verticalReport.boundingRectLeft - verticalReport.boundingRectWidth) / horizWidth);
                    //Determine if the width of the tape on the two targets appears to be the same
                    tapeWidthScore = ratioToScore(vertWidth / horizHeight);
                    //Determine if the vertical location of the horizontal target appears to be correct
                    verticalScore = ratioToScore(1 - (verticalReport.boundingRectTop - horizontalReport.center_mass_y) / (4 * horizHeight));
                    total = leftScore > rightScore ? leftScore : rightScore;
                    total += tapeWidthScore + verticalScore;

                    //If the target is the best detected so far store the information about it
                    if (total > target.totalScore) {
                        target.horizontalIndex = horizontalTargets[j];
                        target.verticalIndex = verticalTargets[i];
                        target.totalScore = total;
                        target.leftScore = leftScore;
                        target.rightScore = rightScore;
                        target.tapeWidthScore = tapeWidthScore;
                        target.verticalScore = verticalScore;
                    }
                }
            }

            if (verticalTargetCount > 0) {
                ParticleAnalysisReport distanceReport = filteredImage.getParticleAnalysisReport(target.verticalIndex);
                distance = computeDistance(filteredImage, distanceReport, target.verticalIndex, true);
            }
        }
        
        filteredImage.free();
        thresholdImage.free();
        image.free();
        
        return distance;
    }
    public void initDefaultCommand() {
        //setDefaultCommand(new DetectTargets());
    }
}